#include "FreeRTOS_Task.h"

// 遥控连接状态
Remote_Status remote_status = UNCONNECTED;
// 飞机的工作状态
Flight_Status flight_status = IDEL;
// 遥控数据
Remote_Struct remote_data = {0};

// 飞行高度
uint16_t height     = 0;
uint16_t fix_height = 0;
// LED结构体
LED_Struct led_left_top     = {LED_LEFT_TOP_GPIO_Port, LED_LEFT_TOP_Pin, 0};
LED_Struct led_left_bottom  = {LED_LEFT_BOTTOM_GPIO_Port, LED_LEFT_BOTTOM_Pin, 0};
LED_Struct led_right_top    = {LED_RIGHT_TOP_GPIO_Port, LED_RIGHT_TOP_Pin, 0};
LED_Struct led_right_bottom = {LED_RIGHT_BOTTOM_GPIO_Port, LED_RIGHT_BOTTOM_Pin, 0};

// 电源任务
void power_task(void *pvParameters);
#define POWER_TASK_NAME       "power_task"
#define POWER_TASK_STACK_SIZE 128
#define POWER_TASK_PRIORITY   4
TaskHandle_t power_task_handle;
#define POWER_TASK_PERIOD 10000

// 灯控任务
void led_task(void *pvParameters);
#define LED_TASK_NAME       "led_task"
#define LED_TASK_STACK_SIZE 128
#define LED_TASK_PRIORITY   1
TaskHandle_t led_task_handle;
#define LED_TASK_PERIOD 50

// 飞控任务
void flight_task(void *pvParameters);
#define FLIGHT_TASK_NAME       "flight_task"
#define FLIGHT_TASK_STACK_SIZE 256
#define FLIGHT_TASK_PRIORITY   3
TaskHandle_t flight_task_handle;
#define FLIGHT_TASK_PERIOD 6

// 通信任务
void communication_task(void *pvParameters);
#define COMMUNICATION_TASK_NAME       "communication_task"
#define COMMUNICATION_TASK_STACK_SIZE 256
#define COMMUNICATION_TASK_PRIORITY   3
TaskHandle_t communication_task_handle;
#define COMMUNICATION_TASK_PERIOD 6

void FreeRTOS_Task_Start(void)
{
    // 初始化VL53L1X
    Int_VL53L1X_Init();
    // 初始化电机
    Int_Motor_Init();

    // 初始化SI24R1
    Int_SI24R1_Init();

    // 初始化MPU6050
    Int_MPU6050_Init();

    // 创建电源任务
    xTaskCreate(power_task, POWER_TASK_NAME, POWER_TASK_STACK_SIZE, NULL, POWER_TASK_PRIORITY, &power_task_handle);

    // 创建灯控任务
    xTaskCreate(led_task, LED_TASK_NAME, LED_TASK_STACK_SIZE, NULL, LED_TASK_PRIORITY, &led_task_handle);

    // 创建飞控任务
    xTaskCreate(flight_task, FLIGHT_TASK_NAME, FLIGHT_TASK_STACK_SIZE, NULL, FLIGHT_TASK_PRIORITY, &flight_task_handle);

    // 创建通信任务
    xTaskCreate(communication_task, COMMUNICATION_TASK_NAME, COMMUNICATION_TASK_STACK_SIZE, NULL, COMMUNICATION_TASK_PRIORITY, &communication_task_handle);

    // 启动调度器
    vTaskStartScheduler();
}

void power_task(void *pvParameters)
{
    uint32_t pre_tick = xTaskGetTickCount();
    while (1) {
        if (ulTaskNotifyTake(pdTRUE, POWER_TASK_PERIOD) == pdTRUE) {
            Int_IP5305T_Stop();
        } else {
            Int_IP5305T_Start();
        }
    }
}

void led_task(void *pvParameters)
{
    uint32_t pre_tick = xTaskGetTickCount();
    while (1) {

        switch (remote_status) {
            case UNCONNECTED:
                Int_LED_Off(&led_left_top);
                Int_LED_Off(&led_right_top);
                break;
            case CONNECTED:
                Int_LED_On(&led_left_top);
                Int_LED_On(&led_right_top);
                break;
            default:
                break;
        }

        switch (flight_status) {
            case IDEL:
                Int_LED_Blink(&led_left_bottom, 1000);
                Int_LED_Blink(&led_right_bottom, 1000);
                break;
            case NORMAL:
                Int_LED_Blink(&led_left_bottom, 100);
                Int_LED_Blink(&led_right_bottom, 100);
                break;
            case FIX_HEIGHT:
                Int_LED_On(&led_left_bottom);
                Int_LED_On(&led_right_bottom);
                break;
            case FAULT:
                Int_LED_Off(&led_left_bottom);
                Int_LED_Off(&led_right_bottom);
                break;
            default:
                break;
        }
        vTaskDelayUntil(&pre_tick, LED_TASK_PERIOD);
    }
}

void flight_task(void *pvParameters)
{
    AccelGyro_Struct accel_gyro   = {0};
    EulerAngle_Struct euler_angle = {0};
    uint32_t pre_tick             = xTaskGetTickCount();

    while (1) {
        // 1.获取六轴数据
        App_Flight_ReadAccelGyro(&accel_gyro);
        // printf("accel: %d, %d, %d\n", accel_gyro.accel.x, accel_gyro.accel.y, accel_gyro.accel.z);
        // printf("gyro: %d, %d, %d\n", accel_gyro.gyro.x, accel_gyro.gyro.y, accel_gyro.gyro.z);

        // 2.获取欧拉角
        App_Flight_GetEulerAngle(&accel_gyro, &euler_angle, FLIGHT_TASK_PERIOD / 1000.0);
        // printf("%f,%f,%f\n", euler_angle.pitch, euler_angle.roll, euler_angle.yaw);

        // 3.角度PID计算
        App_Flight_EulerAnglePID(&remote_data, &euler_angle, &accel_gyro, FLIGHT_TASK_PERIOD / 1000.0);

        // 4.获取高度
        Int_VL53L1X_GetDistance(&height);
        printf("%d\n", height);

        // 5.高度PID计算
        App_Filght_PID(height,fix_height,FLIGHT_TASK_PERIOD / 1000.0);
        // 设置电机速度
        App_Flight_SetMotorSpeed(&remote_data, flight_status);
        vTaskDelayUntil(&pre_tick, FLIGHT_TASK_PERIOD);
    }
}

void communication_task(void *pvParameters)
{
    uint32_t pre_tick = xTaskGetTickCount();

    while (1) {
        // 1.接收数据
        Com_Type isReceived = App_Communication_ReceiveData(&remote_data);

        // 2.处理关机信号
        if (isReceived == Com_SUCCESS) {
            if (remote_data.power_down == 1) {
                xTaskNotifyGive(power_task_handle);
            }
        }

        // 3.处理遥控连接状态
        App_Communication_HandleRemoteStatus(isReceived, &remote_status);

        // 4.处理飞机的工作状态
        App_Communication_HandleFlightStatus(remote_status, &remote_data, &flight_status);

        vTaskDelayUntil(&pre_tick, COMMUNICATION_TASK_PERIOD);
    }
}
